
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_sh3001.c
  * @author     baiyang
  * @date       2023-2-20
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu_sh3001.h"
#include "sensor_imu_sh3001_registers.h"

#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define SH3001_BACKEND_SAMPLE_RATE   1000
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_imu_sh3001_start(sensor_imu_backend *backend);
static bool sensor_imu_sh3001_update(sensor_imu_backend *backend);
static uint8_t register_read(sensor_imu_sh3001_t sh3001, uint8_t reg);
static inline bool block_read(sensor_imu_sh3001_t sh3001, uint8_t reg, uint8_t *buf, uint32_t size);
static bool register_write(sensor_imu_sh3001_t sh3001, uint8_t reg, uint8_t val);
static bool check_whoami(sensor_imu_sh3001_t sh3001);
static bool accel_init(sensor_imu_sh3001_t sh3001);
static bool gyro_init(sensor_imu_sh3001_t sh3001);
static bool init(sensor_imu_sh3001_t sh3001);
static void read_sample(void *parameter);
/*----------------------------------variable----------------------------------*/
static struct sensor_imu_backend_ops sh3001_ops = {.sensor_imu_backend_destructor = NULL,
                                                   .update = sensor_imu_sh3001_update,
                                                   .accumulate = NULL,
                                                   .start = sensor_imu_sh3001_start,
                                                   .get_output_banner = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_sh3001_ctor(sensor_imu_sh3001_t sh3001,
                                    gp_device_t dev,
                                    enum RotationEnum rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(sh3001, 0, sizeof(struct sensor_imu_sh3001));

    sensor_imu_backend_ctor(&sh3001->backend, &sh3001_ops, "sh3001");

    sh3001->_rotation = rotation;
    sh3001->_dev = dev;
}

sensor_imu_backend * sensor_imu_sh3001_probe(gp_device_t dev, enum RotationEnum rotation)
{
    if (!dev) {
        return NULL;
    }

    sensor_imu_sh3001_t sensor = (sensor_imu_sh3001_t)rt_malloc(sizeof(struct sensor_imu_sh3001));

    if (!sensor) {
        return NULL;
    }

    sensor_imu_sh3001_ctor(sensor, dev, rotation);

    if (!init(sensor)) {
        sensor_imu_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_imu_backend *)sensor;
}

static void sensor_imu_sh3001_start(sensor_imu_backend *backend)
{
    sensor_imu_sh3001_t sh3001 = (sensor_imu_sh3001_t)backend;

    if (!sensor_imu_register_accel(&sh3001->_accel_instance, SH3001_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(sh3001->_dev, DEVTYPE_INS_SH3001)) ||
        !sensor_imu_register_gyro(&sh3001->_gyro_instance, SH3001_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(sh3001->_dev, DEVTYPE_INS_SH3001))) {
        return;
    }

    // setup sensor rotations from probe()
    sensor_imu_backend_set_gyro_orientation(sh3001->_gyro_instance, sh3001->_rotation);
    sensor_imu_backend_set_accel_orientation(sh3001->_accel_instance, sh3001->_rotation);
    
    // setup callbacks
    devmgr_register_periodic_callback(sh3001->_dev, 1000000UL / SH3001_BACKEND_SAMPLE_RATE, read_sample, sh3001);
}

static bool sensor_imu_sh3001_update(sensor_imu_backend *backend)
{
    sensor_imu_sh3001_t sh3001 = (sensor_imu_sh3001_t)backend;

    sensor_imu_backend_update_accel(backend, sh3001->_accel_instance);
    sensor_imu_backend_update_gyro(backend, sh3001->_gyro_instance);
    return true;
}

static uint8_t register_read(sensor_imu_sh3001_t sh3001, uint8_t reg)
{
    uint8_t val = 0;

    devmgr_read_registers(sh3001->_dev, reg, &val, 1);

    return val;
}

static inline bool block_read(sensor_imu_sh3001_t sh3001, uint8_t reg, uint8_t *buf, uint32_t size)
{
    return devmgr_read_registers(sh3001->_dev, reg, buf, size);
}

static bool register_write(sensor_imu_sh3001_t sh3001, uint8_t reg, uint8_t val)
{
    uint8_t v2 = 0;
    for (uint8_t i=0; i<8; i++) {
        devmgr_write_register(sh3001->_dev, reg, val, false);
        
        rt_thread_mdelay(1);
        if (reg == SH3001_GYRO_CONF3
            || reg == SH3001_GYRO_CONF4
            || reg == SH3001_GYRO_CONF5
            || (devmgr_read_registers(sh3001->_dev, reg, &v2, 1) && ((v2&val) == val))) {
            return true;
        }
    }

    
    return false;
}

/*
  check whoami for sensor type
 */
static bool check_whoami(sensor_imu_sh3001_t sh3001)
{
    uint8_t whoami = 0;

    // 多次读取以等待硬件自动识别使用的通信接口（spi/i2c）
    for (uint8_t i = 0; i < 3; i++) {
        rt_thread_mdelay(10);
        whoami = register_read(sh3001, SH3001_CHIP_ID);

        if (whoami == SH3001_CHIP_ID_VAL) {
            return true;
        }
    }

    console_printf("[SH3001]: check_whoami fail(0x%X-0x%X)\n", SH3001_CHIP_ID_VAL, whoami);

    // not a value WHOAMI result
    return false;
}

static bool accel_init(sensor_imu_sh3001_t sh3001)
{
    //启用数字滤波
    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_ACC_CONF0, 0x01)) {
        return false;
    }

    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_ACC_CONF1, SH3001_ODR_2000HZ)) {
        return false;
    }

    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_ACC_CONF2, SH3001_ACC_RANGE_16G)) {
        return false;
    }

    sh3001->_accel_scale = GRAVITY_MSS * 16.0f /32768.0f;

    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_ACC_CONF3, SH3001_ACC_ODRX011)) {
        return false;
    }

    // 配置温度寄存器
    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_TEMP_CONF0, SH3001_TEMP_ODR_63 | SH3001_TEMP_EN)) {
        return false;
    }

    // 读取温度偏移量
    rt_thread_mdelay(1);
    uint8_t t1 = register_read(sh3001, SH3001_TEMP_CONF0);

    rt_thread_mdelay(1);
    uint8_t t2 = register_read(sh3001, SH3001_TEMP_CONF1);
    sh3001->_temperature_offset = (int16_t)((uint16_t)(t2 | ((uint16_t)(t1&0x0F) << 8)));

    return true;
}

/*
  probe and initialise gyro
 */
static bool gyro_init(sensor_imu_sh3001_t sh3001)
{
    //启用数字滤波
    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_GYRO_CONF0, 0x01)) {
        return false;
    }

    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_GYRO_CONF1, SH3001_ODR_2000HZ)) {
        return false;
    }

    // 设置截止频率
    //    | 32k  | 16k  | 8k   | 4k   | 1000 | 500 | 250 | 125 | 63 | 31
    // 00 | 2000 | 1600 | 1313 | 1138 | 363  | 181 | 90  | 45  | 23 | 11
    // 01 | 1525 | 1000 | 438  | 438  | 320  | 160 | 80  | 40  | 20 | 10
    // 10 | 1138 | 638  | 313  | 313  | 250  | 125 | 63  | 31  | 15 | 8
    // 11 | 863  | 438  | 213  | 219  | 200  | 100 | 50  | 25  | 13 | 6
    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_GYRO_CONF2, 0x02 << 2)) {
        return false;
    }

    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_GYRO_CONF3, SH3001_GYRO_RANGE_2000)) {
        return false;
    }

    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_GYRO_CONF4, SH3001_GYRO_RANGE_2000)) {
        return false;
    }

    rt_thread_mdelay(1);
    if (!register_write(sh3001, SH3001_GYRO_CONF5, SH3001_GYRO_RANGE_2000)) {
        return false;
    }

    sh3001->_gyro_scale = radians(2000.0f)/32768.0f;

    return true;
}

static bool init(sensor_imu_sh3001_t sh3001)
{
    if (devmgr_get_bus_type(sh3001->_dev) == BUS_TYPE_SPI) {
        devmgr_set_read_flag(sh3001->_dev, 0x80);
    }

    // initially run the bus at low speed
    devmgr_set_speed(sh3001->_dev, DEV_SPEED_LOW);

    if (!check_whoami(sh3001)) {
        return false;;
    }

    bool success = accel_init(sh3001) && gyro_init(sh3001);

    if (success) {
        devmgr_set_speed(sh3001->_dev, DEV_SPEED_HIGH);
        devmgr_set_device_type(sh3001->_dev, DEVTYPE_INS_SH3001);

        console_printf("SH3001 found on bus 0x%x\n", (unsigned)devmgr_get_bus_id(sh3001->_dev));

        rt_thread_mdelay(10);
    }

    return success;
}

/*
  read qmi8610
 */
static void read_sample(void *parameter)
{
    sensor_imu_sh3001_t sh3001 = (sensor_imu_sh3001_t)parameter;

    uint8_t data[sizeof(uint16_t) * 6];
    if (!block_read(sh3001, SH3001_ACC_XL, (uint8_t *)&data[0], sizeof(data))) {
        sensor_imu_backend_inc_accel_error_count(sh3001->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(sh3001->_gyro_instance);
        return;
    }

    Vector3f_t accel = {(int16_t)((uint16_t)(data[0] | (data[1]<<8))) * sh3001->_accel_scale,
                        (int16_t)((uint16_t)(data[2] | (data[3]<<8))) * sh3001->_accel_scale,
                        (int16_t)((uint16_t)(data[4] | (data[5]<<8))) * sh3001->_accel_scale};

    Vector3f_t gyro = {(int16_t)((uint16_t)(data[6] | (data[7]<<8))) * sh3001->_gyro_scale,
                       (int16_t)((uint16_t)(data[8] | (data[9]<<8))) * sh3001->_gyro_scale,
                       (int16_t)((uint16_t)(data[10] | (data[11]<<8))) * sh3001->_gyro_scale};

    sensor_imu_backend_rotate_and_correct_accel(&sh3001->backend, sh3001->_accel_instance, &accel);
    sensor_imu_backend_notify_new_accel_raw_sample(&sh3001->backend, sh3001->_accel_instance, &accel, 0, false);

    sensor_imu_backend_rotate_and_correct_gyro(&sh3001->backend, sh3001->_gyro_instance, &gyro);
    sensor_imu_backend_notify_new_gyro_raw_sample(&sh3001->backend, sh3001->_gyro_instance, &gyro, 0);

    if (sh3001->_temperature_counter++ == 100) {
        sh3001->_temperature_counter = 0;
        uint8_t t[2];
        if (!block_read(sh3001, SH3001_TEMP_ZL, (uint8_t *)&t, sizeof(t))) {
            sensor_imu_backend_inc_accel_error_count(sh3001->_accel_instance);
            sensor_imu_backend_inc_gyro_error_count(sh3001->_gyro_instance);
        } else {
            int16_t tval = (int16_t)((uint16_t)(t[0] | ((uint16_t)(t[1] & 0x0F) << 8)));
            float temp_degc = (tval - sh3001->_temperature_offset) / 16.0f + 25;
            sensor_imu_backend_publish_temperature(&sh3001->backend, sh3001->_accel_instance, temp_degc);
        }
    }

    struct checkreg reg;
    if (!devmgr_check_next_register2(sh3001->_dev, &reg)) {
        //log_register_change(dev_accel->get_bus_id(), reg);
        sensor_imu_backend_inc_accel_error_count(sh3001->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(sh3001->_gyro_instance);
    }
}

/*------------------------------------test------------------------------------*/


